import serial
from binascii import unhexlify
import pymodbus.utilities as uti
import redis
import time
import statistics

if __name__ == '__main__':
    r = redis.Redis(db=0)
    server_r = redis.Redis(db=0, password='lwj380279011', host='117.185.46.193', socket_timeout=1)
    # ser = serial.Serial('/dev/ttyUSB0', 115200)
    # ser.readline()

    distance_l = []

    timestamp = time.time()

    handshake_timestamp = time.time()

    status = 'stop'

    time_pulse = 1
    command = serial.Serial('/dev/ttyUSB3')

    while r.get('process_1').decode("utf-8") == 'on':
        if r.get('radar_1').decode("utf-8") == 'on':
            # result = ser.readline()
            # if len(result) == 19:
            #     result = result[1:-2]
            #
            #     checksum = result[-4:]
            #     data = result[:-4]
            #
            #     try:
            #         if uti.computeCRC(unhexlify(data)) == int(checksum, 16):
            #             distance = int(data[-4:], 16)
            #
            #             distance_l.append(distance)
            #
            #             if len(distance_l) > 40:
            #                 distance_l.pop(0)

            if time.time() - timestamp > time_pulse:
                height = statistics.mean([int(x) for x in r.lrange('distance_q', -10, -1)])
                height_p = statistics.mean([int(x) for x in r.lrange('distance_q', 0, 10)])
                print(height)

                threshold = int(r.get('threshold'))
                limit = 10

                p = height - threshold
                d = height - height_p

                px = 2
                dx = 12

                deviation = p * px + d * dx

                print(deviation)

                # if time_pulse < 1:
                #     time_pulse = 1
                # elif time_pulse > 3:
                #     time_pulse = 3

                if status != 'stop':
                    status = 'stop'
                    command.write(b'*MOTORA2=#')
                    print('stop')
                    time_pulse = 0.5
                elif deviation > limit:
                    status = 'down'
                    # r.set('keyboard_input', 'z')
                    command.write(b'*MOTORA2+#')
                    print('down')
                    time_pulse = 1.6
                elif deviation < 0:
                    status = 'up'
                    # r.set('keyboard_input', 'c')
                    command.write(b'*MOTORA2-#')
                    print('up')
                    time_pulse = 0.8
                else:
                    # r.set('keyboard_input', 'x')
                    status = 'stop'
                    command.write(b'*MOTORA2=#')
                    print('stop')

                timestamp = time.time()
                # except:
                #     print('error')

            # else:
            #     time.sleep(1)
            #     distance_l = []
            #     status = 'stop'

        if time.time() - handshake_timestamp > 1.2:
            try:
                status = server_r.get('pole_status').decode("utf-8")

                command.write(b'*ASK#')
                r.set('process_1_status', 'H.B.')
                print('asmaa')
            except:
                r.set('process_1_status', 'error')
                print('error')
            handshake_timestamp = time.time()

        if r.llen('command') > 0:
            command.write(r.lpop('command'))
            time.sleep(0.1)


